/*
 * Copyright (C) 2009, Willow Garage, Inc.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the names of Stanford University or Willow Garage, Inc. nor the
 * names of its contributors may be used to endorse or promote products derived
 * from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "ros/node_handle.h"
#include "ros/this_node.h"
#include "ros/service.h"
#include "ros/callback_queue.h"

#include "ros/time.h"
#include "ros/rate.h"
#include "ros/timer.h"
#include "ros/wall_timer.h"
#include "ros/steady_timer.h"

#include "ros/xmlrpc_manager.h"
#include "ros/topic_manager.h"
#include "ros/service_manager.h"
#include "ros/master.h"
#include "ros/param.h"
#include "ros/names.h"
#include "ros/init.h"
#include "ros/this_node.h"
#include "xmlrpcpp/XmlRpc.h"

#include <boost/thread.hpp>

namespace ros {

boost::mutex g_nh_refcount_mutex;
int32_t      g_nh_refcount        = 0;
bool         g_node_started_by_nh = false;

class NodeHandleBackingCollection {
 public:
  typedef std::vector<Publisher::ImplWPtr>     V_PubImpl;
  typedef std::vector<ServiceServer::ImplWPtr> V_SrvImpl;
  typedef std::vector<Subscriber::ImplWPtr>    V_SubImpl;
  typedef std::vector<ServiceClient::ImplWPtr> V_SrvCImpl;
  V_PubImpl                                    pubs_;
  V_SrvImpl                                    srvs_;
  V_SubImpl                                    subs_;
  V_SrvCImpl                                   srv_cs_;

  boost::mutex                                 mutex_;

  //  keep shared_ptrs to these managers to avoid assertions. Fixes #838
  TopicManagerPtr   keep_alive_topic_manager   = TopicManager::instance( );
  ServiceManagerPtr keep_alive_service_manager = ServiceManager::instance( );
};

NodeHandle::NodeHandle( const std::string& ns, const M_string& remappings )
    : namespace_( this_node::getNamespace( ) )
    , callback_queue_( 0 )
    , collection_( 0 ) {
  std::string tilde_resolved_ns;
  if ( !ns.empty( ) && ns[0] == '~' )  //  starts with tilde
    tilde_resolved_ns = names::resolve( ns );
  else
    tilde_resolved_ns = ns;

  construct( tilde_resolved_ns, true );

  initRemappings( remappings );
}

NodeHandle::NodeHandle( const NodeHandle& parent, const std::string& ns )
    : collection_( 0 ) {
  namespace_             = parent.getNamespace( );
  callback_queue_        = parent.callback_queue_;

  remappings_            = parent.remappings_;
  unresolved_remappings_ = parent.unresolved_remappings_;

  construct( ns, false );
}

NodeHandle::NodeHandle( const NodeHandle&  parent,
                        const std::string& ns,
                        const M_string&    remappings )
    : collection_( 0 ) {
  namespace_             = parent.getNamespace( );
  callback_queue_        = parent.callback_queue_;

  remappings_            = parent.remappings_;
  unresolved_remappings_ = parent.unresolved_remappings_;

  construct( ns, false );

  initRemappings( remappings );
}

NodeHandle::NodeHandle( const NodeHandle& rhs )
    : collection_( 0 ) {
  callback_queue_        = rhs.callback_queue_;
  remappings_            = rhs.remappings_;
  unresolved_remappings_ = rhs.unresolved_remappings_;

  construct( rhs.namespace_, true );

  unresolved_namespace_ = rhs.unresolved_namespace_;
}

NodeHandle::~NodeHandle( ) {
  destruct( );
}

NodeHandle&
NodeHandle::operator= ( const NodeHandle& rhs ) {
  ROS_ASSERT( collection_ );
  namespace_             = rhs.namespace_;
  callback_queue_        = rhs.callback_queue_;
  remappings_            = rhs.remappings_;
  unresolved_remappings_ = rhs.unresolved_remappings_;

  return *this;
}

void
spinThread( ) {
  ros::spin( );
}

void
NodeHandle::construct( const std::string& ns, bool validate_name ) {
  if ( !ros::isInitialized( ) ) {
    ROS_FATAL(
      "You must call ros::init() before creating the first NodeHandle" );
    ROS_BREAK( );
  }

  collection_           = new NodeHandleBackingCollection;
  unresolved_namespace_ = ns;
  //  if callback_queue_ is nonnull, we are in a non-nullary constructor

  if ( validate_name )
    namespace_ = resolveName( ns, true );
  else {
    namespace_ = resolveName( ns, true, no_validate( ) );
    //  FIXME validate namespace_ now
  }
  ok_ = true;

  boost::mutex::scoped_lock lock( g_nh_refcount_mutex );

  if ( g_nh_refcount == 0 && !ros::isStarted( ) ) {
    g_node_started_by_nh = true;
    ros::start( );
  }

  ++g_nh_refcount;
}

void
NodeHandle::destruct( ) {
  delete collection_;

  boost::mutex::scoped_lock lock( g_nh_refcount_mutex );

  --g_nh_refcount;

  if ( g_nh_refcount == 0 && g_node_started_by_nh ) {
    ros::shutdown( );
  }
}

void
NodeHandle::initRemappings( const M_string& remappings ) {
  {
    M_string::const_iterator it  = remappings.begin( );
    M_string::const_iterator end = remappings.end( );
    for ( ; it != end; ++it ) {
      const std::string& from = it->first;
      const std::string& to   = it->second;

      remappings_.insert( std::make_pair( resolveName( from, false ),
                                          resolveName( to, false ) ) );
      unresolved_remappings_.insert( std::make_pair( from, to ) );
    }
  }
}

void
NodeHandle::setCallbackQueue( CallbackQueueInterface* queue ) {
  callback_queue_ = queue;
}

std::string
NodeHandle::remapName( const std::string& name ) const {
  std::string resolved = resolveName( name, false );

  //  First search any remappings that were passed in specifically for this
  //  NodeHandle
  M_string::const_iterator it = remappings_.find( resolved );
  if ( it != remappings_.end( ) ) {
    //  ROSCPP_LOG_DEBUG("found 'local' remapping: %s", it->second.c_str());
    return it->second;
  }

  //  If not in our local remappings, perhaps in the global ones
  return names::remap( resolved );
}

std::string
NodeHandle::resolveName( const std::string& name, bool remap ) const {
  //  ROSCPP_LOG_DEBUG("resolveName(%s, %s)", name.c_str(), remap ? "true" :
  //  "false");
  std::string error;
  if ( !names::validate( name, error ) ) {
    throw InvalidNameException( error );
  }

  return resolveName( name, remap, no_validate( ) );
}

std::string
NodeHandle::resolveName( const std::string& name,
                         bool               remap,
                         no_validate ) const {
  if ( name.empty( ) ) {
    return namespace_;
  }

  std::string final = name;

  if ( final[0] == '~' ) {
    std::stringstream ss;
    ss << "Using ~ names with NodeHandle methods is not allowed.  If you want "
          "to use private names with the NodeHandle ";
    ss << "interface, construct a NodeHandle using a private name as its "
          "namespace.  e.g. ";
    ss << "ros::NodeHandle nh(\"~\");  ";
    ss << "nh.getParam(\"my_private_name\");";
    ss << " (name = [" << name << "])";
    throw InvalidNameException( ss.str( ) );
  } else if ( final[0] == '/' ) {
    //  do nothing
  } else if ( !namespace_.empty( ) ) {
    //  ROSCPP_LOG_DEBUG("Appending namespace_ (%s)", namespace_.c_str());
    final = names::append( namespace_, final );
  }

  //  ROSCPP_LOG_DEBUG("resolveName, pre-clean: %s", final.c_str());
  final = names::clean( final );
  //  ROSCPP_LOG_DEBUG("resolveName, post-clean: %s", final.c_str());

  if ( remap ) {
    final = remapName( final );
    //  ROSCPP_LOG_DEBUG("resolveName, remapped: %s", final.c_str());
  }

  return names::resolve( final, false );
}

Publisher
NodeHandle::advertise( AdvertiseOptions& ops ) {
  ops.topic = resolveName( ops.topic );
  if ( ops.callback_queue == 0 ) {
    if ( callback_queue_ ) {
      ops.callback_queue = callback_queue_;
    } else {
      ops.callback_queue = getGlobalCallbackQueue( );
    }
  }

  SubscriberCallbacksPtr callbacks(
    boost::make_shared<SubscriberCallbacks>( ops.connect_cb,
                                             ops.disconnect_cb,
                                             ops.tracked_object,
                                             ops.callback_queue ) );

  Publisher pub( ops.topic,
                 ops.md5sum,
                 ops.datatype,
                 ops.latch,
                 *this,
                 callbacks );

  if ( ops.latch ) {
    callbacks->push_latched_message_ = pub.getLastMessageCallback( );
  }

  if ( TopicManager::instance( )->advertise( ops, callbacks ) ) {
    {
      boost::mutex::scoped_lock lock( collection_->mutex_ );
      collection_->pubs_.push_back( pub.impl_ );
    }

    return pub;
  }

  return Publisher( );
}

Subscriber
NodeHandle::subscribe( SubscribeOptions& ops ) {
  ops.topic = resolveName( ops.topic );
  if ( ops.callback_queue == 0 ) {
    if ( callback_queue_ ) {
      ops.callback_queue = callback_queue_;
    } else {
      ops.callback_queue = getGlobalCallbackQueue( );
    }
  }

  if ( TopicManager::instance( )->subscribe( ops ) ) {
    Subscriber sub( ops.topic, *this, ops.helper );

    {
      boost::mutex::scoped_lock lock( collection_->mutex_ );
      collection_->subs_.push_back( sub.impl_ );
    }

    return sub;
  }

  return Subscriber( );
}

ServiceServer
NodeHandle::advertiseService( AdvertiseServiceOptions& ops ) {
  ops.service = resolveName( ops.service );
  if ( ops.callback_queue == 0 ) {
    if ( callback_queue_ ) {
      ops.callback_queue = callback_queue_;
    } else {
      ops.callback_queue = getGlobalCallbackQueue( );
    }
  }

  if ( ServiceManager::instance( )->advertiseService( ops ) ) {
    ServiceServer srv( ops.service, *this );

    {
      boost::mutex::scoped_lock lock( collection_->mutex_ );
      collection_->srvs_.push_back( srv.impl_ );
    }

    return srv;
  }

  return ServiceServer( );
}

ServiceClient
NodeHandle::serviceClient( ServiceClientOptions& ops ) {
  ops.service = resolveName( ops.service );
  ServiceClient client( ops.service, ops.persistent, ops.header, ops.md5sum );

  if ( client ) {
    boost::mutex::scoped_lock lock( collection_->mutex_ );
    collection_->srv_cs_.push_back( client.impl_ );
  }

  return client;
}

Timer
NodeHandle::createTimer( Duration             period,
                         const TimerCallback& callback,
                         bool                 oneshot,
                         bool                 autostart ) const {
  TimerOptions ops;
  ops.period    = period;
  ops.callback  = callback;
  ops.oneshot   = oneshot;
  ops.autostart = autostart;
  return createTimer( ops );
}

Timer
NodeHandle::createTimer( TimerOptions& ops ) const {
  if ( ops.callback_queue == 0 ) {
    if ( callback_queue_ ) {
      ops.callback_queue = callback_queue_;
    } else {
      ops.callback_queue = getGlobalCallbackQueue( );
    }
  }

  Timer timer( ops );
  if ( ops.autostart )
    timer.start( );
  return timer;
}

WallTimer
NodeHandle::createWallTimer( WallDuration             period,
                             const WallTimerCallback& callback,
                             bool                     oneshot,
                             bool                     autostart ) const {
  WallTimerOptions ops;
  ops.period    = period;
  ops.callback  = callback;
  ops.oneshot   = oneshot;
  ops.autostart = autostart;
  return createWallTimer( ops );
}

WallTimer
NodeHandle::createWallTimer( WallTimerOptions& ops ) const {
  if ( ops.callback_queue == 0 ) {
    if ( callback_queue_ ) {
      ops.callback_queue = callback_queue_;
    } else {
      ops.callback_queue = getGlobalCallbackQueue( );
    }
  }

  WallTimer timer( ops );
  if ( ops.autostart )
    timer.start( );
  return timer;
}

SteadyTimer
NodeHandle::createSteadyTimer( WallDuration               period,
                               const SteadyTimerCallback& callback,
                               bool                       oneshot,
                               bool                       autostart ) const {
  SteadyTimerOptions ops;
  ops.period    = period;
  ops.callback  = callback;
  ops.oneshot   = oneshot;
  ops.autostart = autostart;
  return createSteadyTimer( ops );
}

SteadyTimer
NodeHandle::createSteadyTimer( SteadyTimerOptions& ops ) const {
  if ( ops.callback_queue == 0 ) {
    if ( callback_queue_ ) {
      ops.callback_queue = callback_queue_;
    } else {
      ops.callback_queue = getGlobalCallbackQueue( );
    }
  }

  SteadyTimer timer( ops );
  if ( ops.autostart )
    timer.start( );
  return timer;
}

void
NodeHandle::shutdown( ) {
  {
    NodeHandleBackingCollection::V_SubImpl::iterator it
      = collection_->subs_.begin( );
    NodeHandleBackingCollection::V_SubImpl::iterator end
      = collection_->subs_.end( );
    for ( ; it != end; ++it ) {
      Subscriber::ImplPtr impl = it->lock( );

      if ( impl ) {
        impl->unsubscribe( );
      }
    }
  }

  {
    NodeHandleBackingCollection::V_PubImpl::iterator it
      = collection_->pubs_.begin( );
    NodeHandleBackingCollection::V_PubImpl::iterator end
      = collection_->pubs_.end( );
    for ( ; it != end; ++it ) {
      Publisher::ImplPtr impl = it->lock( );

      if ( impl ) {
        impl->unadvertise( );
      }
    }
  }

  {
    NodeHandleBackingCollection::V_SrvImpl::iterator it
      = collection_->srvs_.begin( );
    NodeHandleBackingCollection::V_SrvImpl::iterator end
      = collection_->srvs_.end( );
    for ( ; it != end; ++it ) {
      ServiceServer::ImplPtr impl = it->lock( );

      if ( impl ) {
        impl->unadvertise( );
      }
    }
  }

  {
    NodeHandleBackingCollection::V_SrvCImpl::iterator it
      = collection_->srv_cs_.begin( );
    NodeHandleBackingCollection::V_SrvCImpl::iterator end
      = collection_->srv_cs_.end( );
    for ( ; it != end; ++it ) {
      ServiceClient::ImplPtr impl = it->lock( );

      if ( impl ) {
        impl->shutdown( );
      }
    }
  }

  ok_ = false;
}

void
NodeHandle::setParam( const std::string&         key,
                      const XmlRpc::XmlRpcValue& v ) const {
  return param::set( resolveName( key ), v );
}

void
NodeHandle::setParam( const std::string& key, const std::string& s ) const {
  return param::set( resolveName( key ), s );
}

void
NodeHandle::setParam( const std::string& key, const char* s ) const {
  return param::set( resolveName( key ), s );
}

void
NodeHandle::setParam( const std::string& key, double d ) const {
  return param::set( resolveName( key ), d );
}

void
NodeHandle::setParam( const std::string& key, int i ) const {
  return param::set( resolveName( key ), i );
}

void
NodeHandle::setParam( const std::string& key, bool b ) const {
  return param::set( resolveName( key ), b );
}

void
NodeHandle::setParam( const std::string&              key,
                      const std::vector<std::string>& vec ) const {
  return param::set( resolveName( key ), vec );
}
void
NodeHandle::setParam( const std::string&         key,
                      const std::vector<double>& vec ) const {
  return param::set( resolveName( key ), vec );
}
void
NodeHandle::setParam( const std::string&        key,
                      const std::vector<float>& vec ) const {
  return param::set( resolveName( key ), vec );
}
void
NodeHandle::setParam( const std::string&      key,
                      const std::vector<int>& vec ) const {
  return param::set( resolveName( key ), vec );
}
void
NodeHandle::setParam( const std::string&       key,
                      const std::vector<bool>& vec ) const {
  return param::set( resolveName( key ), vec );
}

void
NodeHandle::setParam( const std::string&                        key,
                      const std::map<std::string, std::string>& map ) const {
  return param::set( resolveName( key ), map );
}
void
NodeHandle::setParam( const std::string&                   key,
                      const std::map<std::string, double>& map ) const {
  return param::set( resolveName( key ), map );
}
void
NodeHandle::setParam( const std::string&                  key,
                      const std::map<std::string, float>& map ) const {
  return param::set( resolveName( key ), map );
}
void
NodeHandle::setParam( const std::string&                key,
                      const std::map<std::string, int>& map ) const {
  return param::set( resolveName( key ), map );
}
void
NodeHandle::setParam( const std::string&                 key,
                      const std::map<std::string, bool>& map ) const {
  return param::set( resolveName( key ), map );
}

bool
NodeHandle::hasParam( const std::string& key ) const {
  return param::has( resolveName( key ) );
}

bool
NodeHandle::deleteParam( const std::string& key ) const {
  return param::del( resolveName( key ) );
}

bool
NodeHandle::getParamNames( std::vector<std::string>& keys ) const {
  return param::getParamNames( keys );
}

bool
NodeHandle::getParam( const std::string& key, XmlRpc::XmlRpcValue& v ) const {
  return param::get( resolveName( key ), v );
}

bool
NodeHandle::getParam( const std::string& key, std::string& s ) const {
  return param::get( resolveName( key ), s );
}

bool
NodeHandle::getParam( const std::string& key, double& d ) const {
  return param::get( resolveName( key ), d );
}

bool
NodeHandle::getParam( const std::string& key, float& f ) const {
  return param::get( resolveName( key ), f );
}

bool
NodeHandle::getParam( const std::string& key, int& i ) const {
  return param::get( resolveName( key ), i );
}

bool
NodeHandle::getParam( const std::string& key, bool& b ) const {
  return param::get( resolveName( key ), b );
}

bool
NodeHandle::getParam( const std::string&        key,
                      std::vector<std::string>& vec ) const {
  return param::get( resolveName( key ), vec );
}
bool
NodeHandle::getParam( const std::string& key, std::vector<double>& vec ) const {
  return param::get( resolveName( key ), vec );
}
bool
NodeHandle::getParam( const std::string& key, std::vector<float>& vec ) const {
  return param::get( resolveName( key ), vec );
}
bool
NodeHandle::getParam( const std::string& key, std::vector<int>& vec ) const {
  return param::get( resolveName( key ), vec );
}
bool
NodeHandle::getParam( const std::string& key, std::vector<bool>& vec ) const {
  return param::get( resolveName( key ), vec );
}

bool
NodeHandle::getParam( const std::string&                  key,
                      std::map<std::string, std::string>& map ) const {
  return param::get( resolveName( key ), map );
}
bool
NodeHandle::getParam( const std::string&             key,
                      std::map<std::string, double>& map ) const {
  return param::get( resolveName( key ), map );
}
bool
NodeHandle::getParam( const std::string&            key,
                      std::map<std::string, float>& map ) const {
  return param::get( resolveName( key ), map );
}
bool
NodeHandle::getParam( const std::string&          key,
                      std::map<std::string, int>& map ) const {
  return param::get( resolveName( key ), map );
}
bool
NodeHandle::getParam( const std::string&           key,
                      std::map<std::string, bool>& map ) const {
  return param::get( resolveName( key ), map );
}

bool
NodeHandle::getParamCached( const std::string&   key,
                            XmlRpc::XmlRpcValue& v ) const {
  return param::getCached( resolveName( key ), v );
}

bool
NodeHandle::getParamCached( const std::string& key, std::string& s ) const {
  return param::getCached( resolveName( key ), s );
}

bool
NodeHandle::getParamCached( const std::string& key, double& d ) const {
  return param::getCached( resolveName( key ), d );
}

bool
NodeHandle::getParamCached( const std::string& key, float& f ) const {
  return param::getCached( resolveName( key ), f );
}

bool
NodeHandle::getParamCached( const std::string& key, int& i ) const {
  return param::getCached( resolveName( key ), i );
}

bool
NodeHandle::getParamCached( const std::string& key, bool& b ) const {
  return param::getCached( resolveName( key ), b );
}

bool
NodeHandle::getParamCached( const std::string&        key,
                            std::vector<std::string>& vec ) const {
  return param::getCached( resolveName( key ), vec );
}
bool
NodeHandle::getParamCached( const std::string&   key,
                            std::vector<double>& vec ) const {
  return param::getCached( resolveName( key ), vec );
}
bool
NodeHandle::getParamCached( const std::string&  key,
                            std::vector<float>& vec ) const {
  return param::getCached( resolveName( key ), vec );
}
bool
NodeHandle::getParamCached( const std::string& key,
                            std::vector<int>&  vec ) const {
  return param::getCached( resolveName( key ), vec );
}
bool
NodeHandle::getParamCached( const std::string& key,
                            std::vector<bool>& vec ) const {
  return param::getCached( resolveName( key ), vec );
}

bool
NodeHandle::getParamCached( const std::string&                  key,
                            std::map<std::string, std::string>& map ) const {
  return param::getCached( resolveName( key ), map );
}
bool
NodeHandle::getParamCached( const std::string&             key,
                            std::map<std::string, double>& map ) const {
  return param::getCached( resolveName( key ), map );
}
bool
NodeHandle::getParamCached( const std::string&            key,
                            std::map<std::string, float>& map ) const {
  return param::getCached( resolveName( key ), map );
}
bool
NodeHandle::getParamCached( const std::string&          key,
                            std::map<std::string, int>& map ) const {
  return param::getCached( resolveName( key ), map );
}
bool
NodeHandle::getParamCached( const std::string&           key,
                            std::map<std::string, bool>& map ) const {
  return param::getCached( resolveName( key ), map );
}

bool
NodeHandle::searchParam( const std::string& key,
                         std::string&       result_out ) const {
  //  searchParam needs a separate form of remapping -- remapping on the
  //  unresolved name, rather than the resolved one.

  std::string              remapped = key;
  M_string::const_iterator it       = unresolved_remappings_.find( key );
  //  First try our local remappings
  if ( it != unresolved_remappings_.end( ) ) {
    remapped = it->second;
  }

  return param::search( resolveName( "" ), remapped, result_out );
}

bool
NodeHandle::ok( ) const {
  return ros::ok( ) && ok_;
}

}  //  namespace ros
